/*
 Slave
 
 Authors: Matthias Robbers and Niels Winkler
 
 This is the sketch for the Arduino board which controls the tank, 
 receives and sends UDP commands via the Ethernet shield and 
 receives wire commands from the second Arduino board (Master).
 This sketch extends the work of the panzer sketch by Dennis 
 Wilhelm and Gerald Pape. 
 */

#include <SPI.h>
#include <Ethernet.h>
#include <SD.h>
#include <Wire.h>

// pins for left and right
int leftEngineOutput = 9;
int rightEngineOutput = 6;
int leftPin1 = 7;
int leftPin2 = 8;
int rightPin1 = 2;
int rightPin2 = 4;

int startDistancePin = 3;

// variable to check if the current moving status of the tank (driving/stopped) 
// is already sent to the smartphone
boolean sent = false;
// global variable to stop the tank before the next command
boolean stop = false;

// IP address of the smartphone controlling the tank
IPAddress remote;

// IP addess, MAC adress and port of the Ethernet shield
byte mac[] = {
  0x06,0x00,0x00,0xb0,0x85,0x04};
byte ip[] = {
  192,168,1,130};
unsigned int localPort = 7777;

// UPD stuff
EthernetUDP Udp;
//buffer to hold incoming packet
char packetBuffer[UDP_TX_PACKET_MAX_SIZE]; 

void setup() {
  // start ethernet connection
  Ethernet.begin(mac,ip);

  // left
  pinMode(leftPin1, OUTPUT);
  pinMode(leftPin2, OUTPUT);
  pinMode(leftEngineOutput, OUTPUT);

  digitalWrite(leftPin1, HIGH);
  digitalWrite(leftPin2, LOW);
  analogWrite(leftEngineOutput, 0);

  // right
  pinMode(rightPin1, OUTPUT);
  pinMode(rightPin2, OUTPUT);  
  pinMode(rightEngineOutput, OUTPUT);  

  digitalWrite(rightPin1, HIGH);
  digitalWrite(rightPin2, LOW);
  analogWrite(rightEngineOutput, 0);  

  // start UDP
  Udp.begin(localPort); 

  // start communication with other board, 
  // i2c bus with address #4
  Wire.begin(4);
  Wire.onReceive(receiveEvent); 

  // start serial monitor
  //  Serial.begin(9600); 

  // give everything some time to initialize
  delay(100);
}

void loop() {
  // local variable to stop the tank before the next command
  boolean stopEngine = false;

  // UPD receive
  int packetSize = Udp.parsePacket();
  if(packetSize) {
    remote = Udp.remoteIP();
    // read the packet into packetBufffer
    Udp.read(packetBuffer,UDP_TX_PACKET_MAX_SIZE);

    String message = "";
    message = (String) packetBuffer;
    String left = "";
    String right = "";
    String direction = "";
    String spin = "";

    //    Serial.print("Raw Message: ");
    //    Serial.println(message);

    // extracting the values for the left and right engine if the 
    // string matches the specification
    if(message.indexOf("ifgi-tank") > -1 && !stop) {
      String sub = message.substring(9, packetSize);
      int semicolon1 = sub.indexOf(";");
      int semicolon2 = sub.indexOf(";", semicolon1 + 1);
      int semicolon3 = sub.lastIndexOf(";");
      // splitting the string into the values for left, right, direction and spin
      left = sub.substring(1, semicolon1);
      right = sub.substring(semicolon1 + 1, semicolon2);
      direction = sub.substring(semicolon2 + 1, semicolon3);
      spin = sub.substring(semicolon3 + 1);
      // converting the Strings to integer values
      char leftChar[5];
      char rightChar[5];
      left.toCharArray(leftChar, 5);
      right.toCharArray(rightChar, 5);

      // map the engine values
      int leftEngine = map(atoi(leftChar), 0, 1023, 0, 255);
      int rightEngine = map(atoi(rightChar), 0, 1023, 0, 255); 

      // stop Engine after receiving "ifgi-tank:0000,0000"
      if (leftEngine == 0  && rightEngine == 0) {
        stopEngine = true;
      } 
      else {
        stopEngine = false;
      }

      // output
      if (direction == "r" && spin == "s") { // reverse and spin
        digitalWrite(leftPin1, LOW);
        digitalWrite(leftPin2, HIGH); 
        digitalWrite(rightPin1, HIGH);
        digitalWrite(rightPin2, LOW);
      } 
      else if (direction == "f" && spin == "s") { // forward and spin
        digitalWrite(leftPin1, HIGH);
        digitalWrite(leftPin2, LOW); 
        digitalWrite(rightPin1, LOW);
        digitalWrite(rightPin2, HIGH);
      } 
      else if (direction == "r" && spin == "f") { // reverse
        digitalWrite(leftPin1, LOW);
        digitalWrite(leftPin2, HIGH); 
        digitalWrite(rightPin1, LOW);
        digitalWrite(rightPin2, HIGH);
      } 
      else { // forward
        digitalWrite(leftPin1, HIGH);
        digitalWrite(leftPin2, LOW); 
        digitalWrite(rightPin1, HIGH);
        digitalWrite(rightPin2, LOW);
      }
      analogWrite(leftEngineOutput, leftEngine);
      analogWrite(rightEngineOutput, rightEngine);
      if (!stopEngine && !sent && remote != NULL) {  
        Udp.beginPacket(remote, 8000);
        Udp.write("start");
        Udp.endPacket();
        sent = true;
      }
    }
  }
  // if there are no new input values or stop variable is set to true 
  // by the other board => stop the engines
  if(stopEngine || stop) {
    analogWrite(leftEngineOutput, 0);
    analogWrite(rightEngineOutput, 0);
    if (remote != NULL) {
      Udp.beginPacket(remote, 8000);
      Udp.write("stop");
      Udp.endPacket();
      sent = false;
    }
  }
  delay(100);
}

// function that executes whenever data is received from master
// this function is registered as an event, see setup()
void receiveEvent(int howMany)
{
  String wireMessage = "";
  while(Wire.available()) // loop through all but the last
  {
    char c = Wire.read(); // receive byte as a character
    //    Serial.print(c);         // print the character
    wireMessage = wireMessage + c;
    if (wireMessage.indexOf(";") > -1) {
      if (wireMessage.indexOf("STOP") > -1) {
        stop = true;
        int colon1 = wireMessage.indexOf(":");
        int semicolon = wireMessage.indexOf(";");
        String where = wireMessage.substring(colon1 + 1, semicolon);
        if (remote != NULL) {
          Udp.beginPacket(remote, 8000);
          if (where == "lf") {
            Udp.write("lf");
          } 
          else if (where == "mf") {
            Udp.write("mf");
          } 
          else if (where == "rf") {
            Udp.write("rf");
          }
          else if (where == "rb") {
            Udp.write("rb");
          }
          else if (where == "mb") {
            Udp.write("mb");
          }
          else if (where == "lb") {
            Udp.write("lb");
          }
          Udp.endPacket();
        }
      } 
      else if (wireMessage.indexOf("START") > -1) {
        stop = false;
        if (remote != NULL) {
          Udp.beginPacket(remote, 8000);
          Udp.write("00");
          Udp.endPacket();
        }
      }
      wireMessage = "";
    }
  }
}